#include "twoservos.h"

TwoServos::TwoServos()
{
}

void TwoServos::setParameters(QList<double> params)
{
    angleMin << params[0] << params[3];
    angleMax << params[1] << params[4];
    length << params[2] << params[5];
}

void TwoServos::addMotor(int freq, double delay, double tau, double imprecision)
{
    motors.append(new Servomotor(this, motors.size(), freq, delay, tau, imprecision));
}

void TwoServos::addSensor(int freq, double delay, double imprecision)
{
    sensors.append(new MotorStateSensor(this, sensors.size(), freq, delay, imprecision));
}

void TwoServos::checkValue(int id)
{
    double angle = motors[id]->getState();

    if(angle > angleMax[id])
    {
        motors[id]->setState(angleMax[id]);
    }
    else if(angle < angleMin[id])
    {
        motors[id]->setState(angleMin[id]);
    }
}

QList<double> TwoServos::getPosition()
{

    double angle1 = (M_PI/180.)*motors[0]->getState(), angle2 = (M_PI/180.)*motors[1]->getState();

    double x1 = length[0]*cos(angle1), y1 = length[0]*sin(angle1);
    double x2 = length[1]*cos(angle1+angle2), y2 = length[1]*sin(angle1+angle2);

    double x = x1+x2;
    double y = y1+y2;

    QList<double> result;
    result << x << y;

    return(result);
}

QList<double> TwoServos::getPositionNormee()
{
    double angle1 = (M_PI/180.)*motors[0]->getState(), angle2 = (M_PI/180.)*motors[1]->getState();

    double x1 = length[0]*cos(angle1), y1 = length[0]*sin(angle1);
    double x2 = length[1]*cos(angle1+angle2), y2 = length[1]*sin(angle1+angle2);
    double lMax = length[0]+length[1];

    double x = (lMax+x1+x2)/(2*lMax);
    double y = (lMax+y1+y2)/(2*lMax);

    QList<double> result;
    result << x << y;

    return(result);
}

double TwoServos::getMotorStateNormee(int id)
{
    return  (motors[id]->getState()-angleMin[id])/(angleMax[id]-angleMin[id]);
}


void TwoServos::setInState(QList<double> state){
    motors[0]->setState(state[0]);
    motors[1]->setState(state[1]);
}
